

#requires the robot team module
require 'control_lib/level1/main/robot.rb'

#requires the robot robotmap module
require 'control_lib/level1/support/robotmap.rb'

#requires the robot log module
require 'control_lib/level1/support/robotlog.rb'

$DEBUG=0

#require the math library
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

################ PROGRAM STARTS HERE ########################

#start the robot
robot=Robot.new("10.1.157.59",3000)

#robot.init("USARBot.P2AT","-6.0,-2.0,1.0","robot1")

#for the DM_Mapping map
robot.init("USARBot.P2AT","0.748, -7.282, -0.446","robot1")
#robot.init("USARBot.P2AT","6.78,11,-0.446","robot2")
robot.recievestart
sleep(2)

#start the robot logging
robotlog=RobotLog.new(robot,true)
robotlog.reset_logs 
robotlog.startlog

#start the map
map=Robotmap.new(robot)
map.configbeliefmap(600,600,0.11,0.11)
map.createbeliefmap
map.autoupdatemap(false)
map.autologmap(true)

robot.colguardmode


cbot=robot
#Main Loop
oldtime=Time.now
totaltime=0.0
checkno=10

while(totaltime<=60.0*19.0)

  puts "totaltime:#{totaltime}"
  #start by backing up a bit
  cbot.smartback(0.2,2.0) if (cbot.col_frontcheck)
  
  #if all wall then U-turn
  sensfront=0.0
  cbot.fSONARSEN.each{|range|
    sensfront=sensfront+1 if range.to_f<1.0
  }
  if(sensfront>=6)
    cbot.smartleft(3.14) 
    puts "U-turning"
  end

  ##                                  ##
  ##      STARTING RRT_MEM METHOD     ##

  #parameters
  w1=1
  w2=2

  #find max from 180 scans
  scans=cbot.rangescan.size
  maxscans=Array.new()
  maxdeg=Array.new()
  scanarray=Array.new(cbot.rangescan)

  #find the max data 
  max=0.0
  maxdist=0.0
  maxdegno=0

  scanarray.each_with_index{|rangestr,deg|
    range=rangestr.to_f
    if(range>maxdist and range<19.9)
      maxdist=range
      maxdegno=deg
    end
   }

  puts "maxdist= #{maxdist}, maxdeg=#{maxdegno}" if($DEBUG==1)

  #make the rest of the data into percent
  scanarrayper=scanarray.collect{|rangestr|
    range=rangestr.to_f
    (range/maxdist)*100
  }

  puts "the scanned percent is #{scanarrayper}"   if($DEBUG==1)

  #Grab the robot INS data
  robotx=cbot.truepos[0]
  roboty=cbot.truepos[1]
  robotang=cbot.truepos[5]

  puts "the robotx=#{robotx},roboty=#{roboty},robotang=#{robotang}" if($DEBUG==1)

  #create an array of the number of unknown points   
  countarray=[]
  maptype=map.mapbel

  #parameters
  scantimes=6.0
  scanarray.each_with_index{|member,rad|
    range=member.to_f
    if(range<19.9)
      #log the white spaces
     scandist= maptype.cellwidth
     frequency=scantimes.to_i
     countno=0
     frequency.times{|sub_range|
       range=range-(sub_range*maptype.cellwidth)
       degree=0.0174*(scans-rad)
       x=range*Math.cos(degree+(robotang.to_f-(3.1415/2)))
       y=range*Math.sin(degree+(robotang.to_f-(3.1415/2)))
       cellpos=maptype.converttocell(x+robotx.to_f,y+roboty.to_f)
       countno=countno+1 if((maptype.isunknown(cellpos[0],cellpos[1],4)))
     } 
     puts "#{countno},#{frequency}" if($DEBUG==1)
     countno=(countno/frequency.to_f)*100
     countarray<<countno
     else
       countarray=0
     end
  }
  
  if($DEBUG==1)
    puts "the count array (#{countarray.size}) is "
    puts countarray 
  end

  #put the weighted array
  scanarray2=[]
  scanarray.size.times{|num|
    scanarray2<<(scanarrayper[num].to_f*w1)+(countarray[num]*w2)
  }

  #The chosen array is   
  if($DEBUG==1)
    puts "The deciding array is "
    puts scanarray2
  end

  #pull out checkno number of data  
  maxrange=[]

  checkno.times{
    max=0.0
    index=-1
    scanarray2.each_with_index{|rangestr,deg|
      fit=rangestr.to_f
      if(fit > max and scanarray[deg].to_f<19.9)
        max=fit
        index=deg
      end
    }
    scanarray2[index]=0 #make min
    maxscans<<max
    maxrange<<scanarray[index].to_f
    maxdeg<<index
  }

  puts "The chosen no is "
  puts maxscans

  #create roulette wheel
  roullet=Array.new(maxscans)

  #find sum
  sum=0.0
  roullet.each{|i|
    sum=sum+i
  }

  #change to percents 
  roullet.collect!{|member|
    member=(member/sum)*100
  }

  #change to sumation of percents
  sum=0.0
  roullet.collect!{|member|
    member=member+sum
    sum=member
  }

  #randomize and choose
  choice=rand(100)
  choicedeg=0.0
  roullet.each_with_index {|member,index|
      if(choice<member)
        choicedeg=maxdeg[index]
        choice=maxrange[index]
        break
      end
  }

  #the chosen deg is choicedeg and distance is choice
  puts "choice is range=#{choice},deg=#{choicedeg}"
  #turn to the chosen distance
  puts de2rad(90-choicedeg)

  if(choicedeg<=90)
    cbot.smartright(de2rad(90-choicedeg))
  else
    cbot.smartleft(de2rad(choicedeg-90))
  end

  while(cbot.fSONARSEN[7].to_f<0.5)
    cbot.smartleft(0.25)
    puts "co: #{cbot.fSONARSEN[7].to_f}"
  end
  while(cbot.fSONARSEN[1].to_f<0.5)
     cbot.smartright(0.25)
     puts "co: #{cbot.fSONARSEN[1].to_f}"
  end
  cbot.smartfor(1.0,2.0)
  #update map 
  puts "updating map"
  cbot.recieve
  map.ruby_maplaserbayes_stand(cbot,0.02,map.mapbel)
  
end  #while

sleep(20)